#include "s3_motor.h"
#include <Ticker.h>

/*
step  A-  B  A  B-
      AN BP AP BN
1     0  0  1  1
2     0  1  1  0
3     1  1  0  0
4     1  0  0  1
*/
uint8_t motor_pos = 0;

uint8_t motor_table[4][4] =
    {
        {0, 0, 1, 1},
        {0, 1, 1, 0},
        {1, 1, 0, 0},
        {1, 0, 0, 1},
    };

Ticker timer_motor;

void timer_motor_callbackfun()
{
    digitalWrite(PIN_MOTOR_AN, motor_table[motor_pos][0]);
    digitalWrite(PIN_MOTOR_BP, motor_table[motor_pos][1]);
    digitalWrite(PIN_MOTOR_AP, motor_table[motor_pos][2]);
    digitalWrite(PIN_MOTOR_BN, motor_table[motor_pos][3]);
    motor_pos++;
    if (motor_pos >= 4)
    {
        motor_pos = 0;
    }
}

void motor_start()
{
    if (timer_motor.active() == false){
        // digitalWrite(PIN_VHEN, HIGH);
        timer_motor.attach_ms(2, timer_motor_callbackfun);
    }
}

void motor_stop()
{
    digitalWrite(PIN_MOTOR_AP, 0);
    digitalWrite(PIN_MOTOR_AN, 0);
    digitalWrite(PIN_MOTOR_BP, 0);
    digitalWrite(PIN_MOTOR_BN, 0);
    // digitalWrite(PIN_VHEN, LOW);
    if (timer_motor.active())
        timer_motor.detach();
}

void motor_run()
{
    digitalWrite(PIN_MOTOR_AN, motor_table[motor_pos][0]);
    digitalWrite(PIN_MOTOR_BP, motor_table[motor_pos][1]);
    digitalWrite(PIN_MOTOR_AP, motor_table[motor_pos][2]);
    digitalWrite(PIN_MOTOR_BN, motor_table[motor_pos][3]);
    motor_pos++;
    if (motor_pos >= 4)
    {
        motor_pos = 0;
    }
    
    // 单步执行后也自动关闭电机
    // 注意：如果需要连续操作，请使用motor_start函数
    delayMicroseconds(4000);  // 保持一段时间让电机移动
    digitalWrite(PIN_MOTOR_AP, 0);
    digitalWrite(PIN_MOTOR_AN, 0);
    digitalWrite(PIN_MOTOR_BP, 0);
    digitalWrite(PIN_MOTOR_BN, 0);
}

void motor_run_step(uint32_t steps)
{
    while (steps)
    {
        digitalWrite(PIN_MOTOR_AN, motor_table[motor_pos][0]);
        digitalWrite(PIN_MOTOR_BP, motor_table[motor_pos][1]);
        digitalWrite(PIN_MOTOR_AP, motor_table[motor_pos][2]);
        digitalWrite(PIN_MOTOR_BN, motor_table[motor_pos][3]);
        motor_pos++;
        if (motor_pos >= 4)
        {
            motor_pos = 0;
        }
        delayMicroseconds(4000);
        steps--;
    }
    
    // 步进完成后关闭电机引脚电源，避免电机长时间通电发热
    digitalWrite(PIN_MOTOR_AP, 0);
    digitalWrite(PIN_MOTOR_AN, 0);
    digitalWrite(PIN_MOTOR_BP, 0);
    digitalWrite(PIN_MOTOR_BN, 0);
}

void motor_run_step_dir(uint32_t steps, bool reverse)
{
    while (steps)
    {
        if (reverse) {
            // 如果是反向，则按照相反的顺序进行
            int pos = motor_pos;
            pos = (pos == 0) ? 3 : pos - 1;
            
            digitalWrite(PIN_MOTOR_AN, motor_table[pos][0]);
            digitalWrite(PIN_MOTOR_BP, motor_table[pos][1]);
            digitalWrite(PIN_MOTOR_AP, motor_table[pos][2]);
            digitalWrite(PIN_MOTOR_BN, motor_table[pos][3]);
            
            motor_pos = pos;
        } else {
            // 正向运行与原函数相同
            digitalWrite(PIN_MOTOR_AN, motor_table[motor_pos][0]);
            digitalWrite(PIN_MOTOR_BP, motor_table[motor_pos][1]);
            digitalWrite(PIN_MOTOR_AP, motor_table[motor_pos][2]);
            digitalWrite(PIN_MOTOR_BN, motor_table[motor_pos][3]);
            
            motor_pos++;
            if (motor_pos >= 4)
            {
                motor_pos = 0;
            }
        }
        
        delayMicroseconds(4000);
        steps--;
    }
    
    // 步进完成后关闭电机引脚电源，避免电机长时间通电发热
    digitalWrite(PIN_MOTOR_AP, 0);
    digitalWrite(PIN_MOTOR_AN, 0);
    digitalWrite(PIN_MOTOR_BP, 0);
    digitalWrite(PIN_MOTOR_BN, 0);
}

void init_motor()
{
    pinMode(PIN_MOTOR_AN, OUTPUT);
    pinMode(PIN_MOTOR_BN, OUTPUT);
    pinMode(PIN_MOTOR_AP, OUTPUT);
    pinMode(PIN_MOTOR_BP, OUTPUT);

    digitalWrite(PIN_MOTOR_AP, 0);
    digitalWrite(PIN_MOTOR_AN, 0);
    digitalWrite(PIN_MOTOR_BP, 0);
    digitalWrite(PIN_MOTOR_BN, 0);
}